

#include "sl_gauss.h"

#include<stdlib.h>
#include<math.h>




/// <summary>
/// 初始化数据。样本数据最好比维度高。这样协方差矩阵才满秩。
/// </summary>
/// <param name="gauss">高斯分布数据的结构体</param>
/// <param name="sample">样本数组</param>
/// <param name="sample_count">样本数量</param>
void gauss_vec_init_by_data(gauss_vec* gauss, vec sample[], int sample_count)
{
    int i, j;
    mat gauss__sample_data;        //样本数据矩阵。每个行矢量是一个样本，有rows个样本。列数等于样本数据的维度。
    mat gauss__sample_shift;    //样本数据偏移矩阵。将样本的中心偏移到原点。每个行矢量是一个样本，有rows个样本。列数等于样本数据的维度。
    mat gauss__sample_shift_T;  //样本数据偏移矩阵的转置。
 //   mat D;
    mat cov;
    int gauss__sample_count = sample_count;  //样本数量

    gauss_vec_init(gauss, sample[0].size);

    mat_allocate(&gauss__sample_data, gauss__sample_count, gauss->dimension);
    mat_allocate(&gauss__sample_shift, gauss__sample_count, gauss->dimension);
    mat_allocate(&gauss__sample_shift_T, gauss->dimension, gauss__sample_count);
    mat_allocate(&cov, gauss->dimension, gauss->dimension);

    for (i = 0; i < gauss__sample_count; i++) {
        for (j = 0; j < gauss->dimension; j++) {
            gauss__sample_data.data[i][j] = sample[i].data[j];
        }
    }

    vec_init(gauss->mean_vector, 0);
    for (i = 0; i < gauss__sample_count; i++) {
        for (j = 0; j < gauss->dimension; j++) {
            gauss->mean_vector.data[j] += gauss__sample_data.data[i][j];
        }
    }
    vec_mul(gauss->mean_vector, 1.0f / sample_count, gauss->mean_vector); //中心点

    //初始化偏移矩阵。
    for (i = 0; i < gauss__sample_data.rows; i++) {
        for (j = 0; j < gauss__sample_data.cols; j++) {
            gauss__sample_shift.data[i][j] = gauss__sample_data.data[i][j] - gauss->mean_vector.data[j];
        }
    }
    mat_transpose_to(gauss__sample_shift, gauss__sample_shift_T);

    //初始化协方差矩阵
    mat_mul(gauss__sample_shift_T, gauss__sample_shift, gauss->covariance);
    mat_copyto(gauss->covariance, cov);
    mat_inversion(cov, gauss->icovariance, Epsilon);

    mat_copyto(gauss->covariance, cov);
    gauss->cov_det = mat_det(cov, Epsilon);
    gauss->sqrt_cov_det = sqrtf(gauss->cov_det);

    // 计算A， A*AT = covariance， 计算方法：特征值分解。
    mat_EVD_Symmetric(gauss->covariance, gauss->A, cov, Epsilon);

    for (int i = 0; i < cov.rows; i++) {
        cov.data[i][i] = sqrtf(cov.data[i][i]);
        for (int j = 0; j < cov.cols; j++)
        {
            gauss->A.data[j][i] *= cov.data[i][i];
        }
    }

    mat_deallocate(&gauss__sample_data);     //样本数据矩阵。每个行矢量是一个样本，有rows个样本。列数等于样本数据的维度。
    mat_deallocate(&gauss__sample_shift);    //样本数据偏移矩阵。将样本的中心偏移到原点。每个行矢量是一个样本，有rows个样本。列数等于样本数据的维度。
    mat_deallocate(&gauss__sample_shift_T);  //样本数据偏移矩阵的转置。
    mat_deallocate(&cov);
}

/// <summary>
/// 初始化数据。样本数据最好比维度高。这样协方差矩阵才满秩。
/// </summary>
/// <param name="gauss">高斯分布数据的结构体</param>
/// <param name="d">样本维度</param>
void gauss_vec_init(gauss_vec* gauss, int d)
{
//    int i, j;
    gauss->dimension = d;

    mat_allocate(&gauss->A, gauss->dimension, gauss->dimension);
    mat_allocate(&gauss->covariance, gauss->dimension, gauss->dimension);
    mat_allocate(&gauss->icovariance, gauss->dimension, gauss->dimension);

    vec_allocate(&gauss->mean_vector, gauss->dimension);
    vec_allocate(&gauss->temp, gauss->dimension);
    vec_allocate(&gauss->random, gauss->dimension);

    vec_init(gauss->mean_vector, 0);
    vec_init(gauss->temp, 0);
    vec_init(gauss->random, 0);

    mat_eye(gauss->A);
    mat_eye(gauss->covariance);
    mat_eye(gauss->icovariance);

    //初始化协方差矩阵
    gauss->cov_det = 1;
    gauss->sqrt_cov_det = sqrtf(gauss->cov_det);
    //


}

/// <summary>
/// 释放高斯分布数据的结构体
/// </summary>
/// <param name="gauss">高斯分布数据的结构体</param>
void gauss_vec_release(gauss_vec* gauss)
{
    mat_deallocate(&gauss->covariance);
    mat_deallocate(&gauss->icovariance);
    mat_deallocate(&gauss->A);
    vec_deallocate(&gauss->mean_vector);
    vec_deallocate(&gauss->random);
    vec_deallocate(&gauss->temp);
}

/// <summary>
/// 高斯分布更新协方差矩阵后，调用这个函数刷新其他变量
/// </summary>
/// <param name="gauss">高斯分布数据的结构体 </param>
void gauss_vec_renew_cov(gauss_vec* gauss)
{
    mat cov;
//    int gauss__sample_count;  //样本数量
    mat_allocate(&cov, gauss->dimension, gauss->dimension);
    //初始化协方差矩阵
    mat_copyto(gauss->covariance, cov);
    mat_inversion(cov, gauss->icovariance, Epsilon);
    mat_copyto(gauss->covariance, cov);
    gauss->cov_det = mat_det(cov, Epsilon);
    gauss->sqrt_cov_det = sqrtf(gauss->cov_det);
    // 计算A， A*AT = covariance， 计算方法：特征值分解。
    mat_EVD_Symmetric(gauss->covariance, gauss->A, cov, Epsilon);
    for (int i = 0; i < cov.rows; i++) {
        cov.data[i][i] = sqrtf(cov.data[i][i]);
        for (int j = 0; j < cov.cols; j++)
        {
            gauss->A.data[j][i] *= cov.data[i][i];
        }
    }
    //
    mat_deallocate(&cov);
}

/// <summary>
/// 计算矢量在数据中的概率。假设概率是高维高斯分布的。
/// </summary>
/// <param name="gauss">高斯分布数据的结构体</param>
/// <param name="v">样本矢量</param>
/// <returns>该样本矢量在数据集中出现的概率，这里的概率是概率密度，需要积分才有意义</returns>
real_t gauss_vec_probability(gauss_vec gauss, vec v)
{
    real_t f;
    vec_sub(v, gauss.mean_vector, gauss.random);
    mat_mul_vec(gauss.icovariance, gauss.random, gauss.temp);

    f = vec_dotProduct(gauss.random, gauss.temp);
    f = powf(EulerNumber, f * (-0.5f));
    //  return 1.0f * f / (powf(PI_mul_2, gauss.dimension * 0.5f) * sqrtf(gauss.cov_det));
    return 1.0f * f / (powf(PI_mul_2, gauss.dimension * 0.5f) * (gauss.sqrt_cov_det));
}

/// <summary>
/// 刷新高斯的随机矢量。运行该函数后，刷新矢量v , gauss.random变成gauss的随机矢量
/// </summary>
/// <param name="gauss">高斯分布的结构体</param>
void gauss_vec_randomize(gauss_vec gauss)
{
    for (int i = 0; i < gauss.dimension; i++) {
        gauss.temp.data[i] = random_gauss();  //随机生成均值0，方差1 的高斯矢量
    }
    mat_mul_vec(gauss.A, gauss.temp, gauss.random);
    vec_add(gauss.mean_vector, gauss.random, gauss.random);
}
//



/// <summary>
/// 计算直方图
/// </summary>
/// <param name="hist">直方图，所有值累加是1</param>
/// <param name="hist_x">直方图等级对应的函数值</param>
/// <param name="hist_len">直方图等级数量</param>
/// <param name="buf">待统计的数组</param>
/// <param name="len">待统计的数组长度</param>
void get_Hist(real_t* hist, real_t* hist_x, int hist_len, real_t* buf, int buf_len)
{
    for (int i = 0; i < hist_len; i++) {
        hist[i] = 0;
    }
    float max = buf[0], min = buf[0];
    for (int i = 0; i < buf_len; i++) {
        max = max > buf[i] ? max : buf[i];
        min = min < buf[i] ? min : buf[i];
    }
    float delta = (max - min) / hist_len;

    int gray;  //灰度等级
    for (int i = 0; i < buf_len; i++) {
        gray = (buf[i] - min) / delta;
        hist[gray]++;
    }
    //归一化
    for (int i = 0; i < hist_len; i++) {
        hist[i] = hist[i] / buf_len;   //这样所有的hist[i]加起来就是 1 了。
        hist_x[i] = min + delta * (i + 0.5);
    }

}

#include"gl_.h"
void test_Gauss()
{
#define array_length   100   //数组长度
    float y1[array_length];
    float y2[array_length];
    float x[array_length];

    //绘制两个随机函数
    for (int i = 0; i < array_length; i++) {
        x[i] = i;
        y1[i] = random_gauss() + 2;
        y2[i] = random_gauss()*2 - 4;
    }

//    int i = 500;
    TwoFunctionPicture tp;
    TwoFunctionPicture_init(&tp, 0, 0, 700, 450);
    //  TwoFunctionPicture_init(&tp, 320, 400);

    char title[] = "Curve Chart";          //标题
    char xtitle[] = "xtitle time";          //x轴标题
    char y1title[] = "y1title cm";          //y轴标题
    char y2title[] = "y2title km";          //y轴标题
    TwoFunctionPicture_draw_title(&tp, title, xtitle, y1title, y2title);
    if (0)
    {
        TwoFunctionPicture_draw_func(&tp,
            x, y1, array_length,
            x, y2, array_length,
            1, 0, 0, NULL, NULL);
        GD_wait_key_input();	// 按任意键继续

        TwoFunctionPicture_draw_func(&tp,
            x, y1, array_length,
            x, y2, array_length,
            0, 1, 0, NULL, NULL);
        GD_wait_key_input();	// 按任意键继续

        TwoFunctionPicture_draw_func(&tp,
            x, y1, array_length,
            x, y2, array_length,
            1, -1, 0, NULL, NULL);
        GD_wait_key_input();	// 按任意键继续

        TwoFunctionPicture_draw_func(&tp,
            x, y1, array_length,
            x, y2, array_length,
            0, -1, 1, NULL, NULL);
        GD_wait_key_input();	// 按任意键继续
    }

    //绘制随机分布点
    COLOR_RGB color[array_length];
    for (int i = 0; i < array_length / 2; i++) {
        x[i] = random_gauss() * 3 + 4;
        y1[i] = random_gauss() * 2 + 1;
        color[i] = RGB_green;
    }
    for (int i = array_length / 2; i < array_length; i++) {
        x[i] = random_gauss() * 3 - 5;
        y1[i] = random_gauss() * 11 + 6;;
        color[i] = RGB_red;
    }
    TwoFunctionPicture_draw_func(&tp,
        x, y1, array_length,
               NULL, NULL, 0,
        0, 0, 0, color, color);
    GD_wait_key_input();	// 按任意键继续

    //绘制高斯分布的直方图
    float hist[array_length];
    float y3[1000];
    for (int i = 0; i < 1000; i++) {
        y3[i] = random_gauss() * 2 + 5;
    }
    int xlen = array_length / 2;
    get_Hist(hist, x, xlen, y3, 1000);
    TwoFunctionPicture_draw_func(&tp,
        x, hist, xlen,
        NULL, NULL, 0,
        0, 0, 0, NULL, NULL);
    GD_wait_key_input();	// 按任意键继续

    xlen = 10;
    get_Hist(hist, x, xlen, y3, 1000);
    TwoFunctionPicture_draw_func(&tp,
        x, hist, xlen,
        NULL, NULL, 0,
        1, 0, 0, NULL, NULL);
    GD_wait_key_input();	// 按任意键继续


}
//
















